#include "NeZha.h"
#include <Wire.h>

NeZha::NeZha() {
  Wire.begin();
}



void NeZha::set_motor_speed(int motor, int speed){
 byte buf[4] = {0, 0, 0, 0};
 if (speed < 0){
  buf[1] = 0x02;
  buf[2] = speed * -1;
 }else{
  buf[1] = 0x01;
  buf[2] = speed;   
 }
 if (motor == 0){
  buf[0] = 0x01;
 }else if (motor == 1){
  buf[0] = 0x02;
 }else if (motor == 2){
  buf[0] = 0x03;
 }else{
  buf[0] = 0x04;
 }
 this->i2cWriteBuf(0x10,buf,4);
}



void NeZha::stop_motor(int motor){
 byte buf[4] = {0, 0x01, 0, 0};
 if (motor == 0){
  buf[0] = 0x01;
 }else if (motor == 1){
  buf[0] = 0x02;
 }else if (motor == 2){
  buf[0] = 0x03;
 }else{
  buf[0] = 0x04;
 }
 this->i2cWriteBuf(0x10,buf,4); 
}



void NeZha::stop_all_motor(void){
 byte buf[4] = {0, 0x01, 0, 0};
 buf[0] = 0x01; 
 this->i2cWriteBuf(0x10,buf,4);
 buf[0] = 0x02; 
 this->i2cWriteBuf(0x10,buf,4);
 buf[0] = 0x03; 
 this->i2cWriteBuf(0x10,buf,4);
 buf[0] = 0x04; 
 this->i2cWriteBuf(0x10,buf,4); 
}



void NeZha::set_servo_angle(int servotype, int servo, int angle){
 byte buf[4] = {0, 0, 0, 0};
 if (servotype == 180){
  if (angle > 180){
    angle = 180;
  }else if (angle < 0){
    angle = 0;                
  }
  buf[1] = angle;
 }else if (servotype == 270){
  if (angle > 225){
    angle = 225;
  }else if (angle < 0){
    angle = 0;  
  }
  buf[1] = round((angle+45)/ 270 * 180);
 }else{
  if (angle > 360){
    angle = 360;
  }else if (angle < 0){
    angle = 0; 
  }
  buf[1] = round(angle/2);
 }
 if (servo == 0){
  buf[0] = 0x10;
 }else if (servo == 1){
  buf[0] = 0x11;
 }else if (servo == 2){
  buf[0] = 0x12;
 }else if (servo == 3){
  buf[0] = 0x13;
 }
 this->i2cWriteBuf(0x10,buf,4);   
}




void NeZha::set_servo_speed(int servo, int speed){
 byte buf[4] = {0, 0, 0, 0};
 if (servo == 0){
  buf[0] = 0x10;
 }else if (servo == 1){
  buf[0] = 0x11;
 }else if (servo == 2){
  buf[0] = 0x12;
 }else if (servo == 3){
  buf[0] = 0x13;
 }
  buf[1] = round((speed + 100)/ 200 * 180);
 this->i2cWriteBuf(0x10,buf,4);  
}




void NeZha::i2cWriteBuf(int addr, unsigned char *p, int len)
{
#ifdef ESP_PLATFORM
  Wire.setClock(100000);
#endif
  Wire.beginTransmission(addr);
  for(int i=0; i<len; i++)
    Wire.write((uint8_t)p[i]);
  Wire.endTransmission();
}
